{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "# HybridEliminationTree"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/hybrid/doc/HybridEliminationTree.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 13,
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass  # Not running on Colab, do nothing"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "A `HybridEliminationTree` is the hybrid equivalent of `gtsam.GaussianEliminationTree`. It represents the structure of **sequential** variable elimination performed on a `HybridGaussianFactorGraph` given a specific `Ordering`.\n",
        "\n",
        "Each node in the tree corresponds to a variable being eliminated. The factors associated with that variable (from the original graph) are stored at that node. The tree structure reflects the dependencies created during sequential elimination: the parent of a node `j` is the variable `i` appearing earliest in the ordering such that the conditional $P(j | ...)$ resulting from eliminating `j` involves `i`.\n",
        "\n",
        "Eliminating a `HybridEliminationTree` yields a `HybridBayesNet`.\n",
        "\n",
        "While fundamental to sequential elimination, direct manipulation of `HybridEliminationTree` objects is less common than using the `eliminateSequential` method on a `HybridGaussianFactorGraph`, which uses this structure internally. It primarily serves to understand the computational flow of sequential elimination in the hybrid context."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 14,
      "metadata": {},
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "import graphviz\n",
        "\n",
        "from gtsam import (\n",
        "    HybridGaussianFactorGraph,\n",
        "    Ordering,\n",
        "    HybridEliminationTree,\n",
        "    JacobianFactor,\n",
        "    DecisionTreeFactor,\n",
        "    HybridGaussianFactor,\n",
        ")\n",
        "from gtsam.symbol_shorthand import X, D"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {},
      "source": [
        "## Creating a HybridEliminationTree\n",
        "\n",
        "It is constructed from a `HybridGaussianFactorGraph` and an `Ordering`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 15,
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Original HybridGaussianFactorGraph:\n"
          ]
        },
        {
          "data": {
            "image/svg+xml": [
              "<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
              "<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
              " \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
              "<!-- Generated by graphviz version 12.0.0 (0)\n",
              " -->\n",
              "<!-- Pages: 1 -->\n",
              "<svg width=\"206pt\" height=\"84pt\"\n",
              " viewBox=\"0.00 0.00 206.00 83.60\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
              "<g id=\"graph0\" class=\"graph\" transform=\"scale(1 1) rotate(0) translate(4 79.6)\">\n",
              "<polygon fill=\"white\" stroke=\"none\" points=\"-4,4 -4,-79.6 202,-79.6 202,4 -4,4\"/>\n",
              "<!-- var7205759403792793600 -->\n",
              "<g id=\"node1\" class=\"node\">\n",
              "<title>var7205759403792793600</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"27\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"27\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">d0</text>\n",
              "</g>\n",
              "<!-- factor0 -->\n",
              "<g id=\"node4\" class=\"node\">\n",
              "<title>factor0</title>\n",
              "<ellipse fill=\"black\" stroke=\"black\" cx=\"27\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
              "</g>\n",
              "<!-- var7205759403792793600&#45;&#45;factor0 -->\n",
              "<g id=\"edge1\" class=\"edge\">\n",
              "<title>var7205759403792793600&#45;&#45;factor0</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M27,-39.28C27,-25.77 27,-8.54 27,-3.96\"/>\n",
              "</g>\n",
              "<!-- factor2 -->\n",
              "<g id=\"node6\" class=\"node\">\n",
              "<title>factor2</title>\n",
              "<ellipse fill=\"black\" stroke=\"black\" cx=\"81\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
              "</g>\n",
              "<!-- var7205759403792793600&#45;&#45;factor2 -->\n",
              "<g id=\"edge4\" class=\"edge\">\n",
              "<title>var7205759403792793600&#45;&#45;factor2</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M41.47,-42.18C55.47,-28.24 75.23,-8.54 79.96,-3.84\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352320 -->\n",
              "<g id=\"node2\" class=\"node\">\n",
              "<title>var8646911284551352320</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"171\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"171\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">x0</text>\n",
              "</g>\n",
              "<!-- factor1 -->\n",
              "<g id=\"node5\" class=\"node\">\n",
              "<title>factor1</title>\n",
              "<ellipse fill=\"black\" stroke=\"black\" cx=\"171\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352320&#45;&#45;factor1 -->\n",
              "<g id=\"edge2\" class=\"edge\">\n",
              "<title>var8646911284551352320&#45;&#45;factor1</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M171,-39.28C171,-25.77 171,-8.54 171,-3.96\"/>\n",
              "</g>\n",
              "<!-- factor3 -->\n",
              "<g id=\"node7\" class=\"node\">\n",
              "<title>factor3</title>\n",
              "<ellipse fill=\"black\" stroke=\"black\" cx=\"117\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352320&#45;&#45;factor3 -->\n",
              "<g id=\"edge5\" class=\"edge\">\n",
              "<title>var8646911284551352320&#45;&#45;factor3</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M156.53,-42.18C142.53,-28.24 122.77,-8.54 118.04,-3.84\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352321 -->\n",
              "<g id=\"node3\" class=\"node\">\n",
              "<title>var8646911284551352321</title>\n",
              "<ellipse fill=\"none\" stroke=\"black\" cx=\"99\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
              "<text text-anchor=\"middle\" x=\"99\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">x1</text>\n",
              "</g>\n",
              "<!-- var8646911284551352321&#45;&#45;factor2 -->\n",
              "<g id=\"edge3\" class=\"edge\">\n",
              "<title>var8646911284551352321&#45;&#45;factor2</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M93.3,-39.58C88.77,-26.03 82.94,-8.61 81.39,-3.97\"/>\n",
              "</g>\n",
              "<!-- var8646911284551352321&#45;&#45;factor3 -->\n",
              "<g id=\"edge6\" class=\"edge\">\n",
              "<title>var8646911284551352321&#45;&#45;factor3</title>\n",
              "<path fill=\"none\" stroke=\"black\" d=\"M104.7,-39.58C109.23,-26.03 115.06,-8.61 116.61,-3.97\"/>\n",
              "</g>\n",
              "</g>\n",
              "</svg>\n"
            ],
            "text/plain": [
              "<graphviz.sources.Source at 0x10aced340>"
            ]
          },
          "execution_count": 15,
          "metadata": {},
          "output_type": "execute_result"
        }
      ],
      "source": [
        "# --- Create a HybridGaussianFactorGraph (same as HybridBayesTree example) ---\n",
        "hgfg = HybridGaussianFactorGraph()\n",
        "dk0 = (D(0), 2) # Binary discrete variable\n",
        "\n",
        "# Prior on D0: P(D0=0)=0.6, P(D0=1)=0.4\n",
        "prior_d0 = DecisionTreeFactor([dk0], \"0.6 0.4\")\n",
        "hgfg.add(prior_d0) # Factor 0\n",
        "\n",
        "# Prior on X0: P(X0) = N(0, 1)\n",
        "prior_x0 = JacobianFactor(X(0), np.eye(1), np.zeros(1), gtsam.noiseModel.Isotropic.Sigma(1, 1.0))\n",
        "hgfg.add(prior_x0) # Factor 1\n",
        "\n",
        "# Conditional measurement on X1: P(X1 | D0)\n",
        "# Mode 0: P(X1 | D0=0) = N(1, 0.25)\n",
        "gf0 = JacobianFactor(X(1), np.eye(1), np.array([1.0]), gtsam.noiseModel.Isotropic.Sigma(1, 0.5))\n",
        "# Mode 1: P(X1 | D0=1) = N(5, 1.0)\n",
        "gf1 = JacobianFactor(X(1), np.eye(1), np.array([5.0]), gtsam.noiseModel.Isotropic.Sigma(1, 1.0))\n",
        "meas_x1_d0 = HybridGaussianFactor(dk0, [gf0, gf1])\n",
        "hgfg.add(meas_x1_d0) # Factor 2\n",
        "\n",
        "# Factor connecting X0 and X1: P(X1 | X0) = N(X0+1, 0.1)\n",
        "odom_x0_x1 = JacobianFactor(X(0), -np.eye(1), X(1), np.eye(1), np.array([1.0]), gtsam.noiseModel.Isotropic.Sigma(1, np.sqrt(0.1)))\n",
        "hgfg.add(odom_x0_x1) # Factor 3\n",
        "\n",
        "print(\"Original HybridGaussianFactorGraph:\")\n",
        "# hgfg.print()\n",
        "graphviz.Source(hgfg.dot())"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 16,
      "id": "26adbd08",
      "metadata": {},
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Elimination Ordering: Position 0: x0, x1, d0\n",
            "\n",
            "\n",
            "Resulting HybridEliminationTree:\n",
            "-(d0)\n",
            "- f[ (d0,2), ]\n",
            " Choice(d0) \n",
            " 0 Leaf  0.6\n",
            " 1 Leaf  0.4\n",
            "| -(x1)\n",
            "| -\n",
            "Hybrid [x1; d0]{\n",
            " Choice(d0) \n",
            " 0 Leaf :\n",
            "  A[x1] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 1 ]\n",
            "isotropic dim=1 sigma=0.5\n",
            "scalar: 0\n",
            "\n",
            " 1 Leaf :\n",
            "  A[x1] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 5 ]\n",
            "  Noise model: unit (1) \n",
            "scalar: 0\n",
            "\n",
            "}\n",
            "| | -(x0)\n",
            "| | -\n",
            "  A[x0] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 0 ]\n",
            "  Noise model: unit (1) \n",
            "| | -\n",
            "  A[x0] = [\n",
            "\t-1\n",
            "]\n",
            "  A[x1] = [\n",
            "\t1\n",
            "]\n",
            "  b = [ 1 ]\n",
            "isotropic dim=1 sigma=0.316228\n"
          ]
        }
      ],
      "source": [
        "\n",
        "# --- Define an Ordering ---\n",
        "# Eliminate continuous first, then discrete (matches default HybridOrdering)\n",
        "ordering = Ordering([X(0), X(1), D(0)])\n",
        "print(f\"\\nElimination Ordering: {ordering}\")\n",
        "\n",
        "# --- Construct the Elimination Tree ---\n",
        "het = HybridEliminationTree(hgfg, ordering)\n",
        "print(\"\\nResulting HybridEliminationTree:\")\n",
        "# Printing shows the tree structure and factors stored at each node\n",
        "het.print()"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "85db36bd",
      "metadata": {},
      "source": [
        "The primary use for elimination trees is in sequential elimination, yielding a `HybridBayesNet`. This is typically done via the `eliminateSequential` method of the factor graph itself."
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 5
}
